#include "dmpHAL.h"

// #define I2C_BUS_DEBUG

int i2c_write(uint8_t slave_addr,uint8_t reg_addr,uint8_t length,const uint8_t *data)
{
    SCB_CleanDCache_by_Addr((uint32_t *)data,length);
    if(HAL_I2C_Mem_Write(&hi2c2,(slave_addr << 1),reg_addr,I2C_MEMADD_SIZE_8BIT,(uint8_t *)data,length,HAL_MAX_DELAY) != HAL_OK) return -1;
    // assert(HAL_I2C_Master_Transmit(&hi2c2,slave_addr << 1,&reg_addr,1,HAL_MAX_DELAY) == HAL_OK);
    // assert(HAL_I2C_Master_Transmit(&hi2c2,slave_addr << 1,data,length,HAL_MAX_DELAY) == HAL_OK);
    #ifdef I2C_BUS_DEBUG
    printf("[I2C_BUS_DEBUG] <i2c_write> Address: %02x Data: ",reg_addr);
    for(int i=0;i<length;i++)
    {
        printf("%02x ",data[i]);
    }
    printf("\r\n");
    #endif
    // assert(HAL_I2C_Mem_Write(&hi2c2,slave_addr,reg_addr,I2C_MEMADD_SIZE_8BIT,data,length,HAL_MAX_DELAY) == HAL_OK);
    return 0;
}

int  i2c_read(uint8_t slave_addr,uint8_t reg_addr,uint8_t length, uint8_t *data)
{
    if(HAL_I2C_Mem_Read(&hi2c2,(slave_addr<<1 | 1),reg_addr,I2C_MEMADD_SIZE_8BIT,data,length,HAL_MAX_DELAY) != HAL_OK) return -1;
    // assert(HAL_I2C_Master_Transmit(&hi2c2,slave_addr << 1,&reg_addr,1,HAL_MAX_DELAY) == HAL_OK);
    // assert(HAL_I2C_Master_Receive(&hi2c2, slave_addr << 1 | 1,data,length,HAL_MAX_DELAY) == HAL_OK);
    #ifdef I2C_BUS_DEBUG
    printf("[I2C_BUS_DEBUG] <i2c_read> Address: %02x Data: ",reg_addr);
    for(int i=0;i<length;i++)
    {
        printf("%02x ",data[i]);
    }
    printf("\r\n");
    #endif
    SCB_InvalidateDCache_by_Addr((uint32_t *)data,length);
    return 0;
}

void delay_ms(uint32_t num_ms)
{
    HAL_Delay(num_ms);
}

void get_ms(uint32_t *count)
{
    *count = HAL_GetTick();
}

